import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.action.server import ServerGoalHandle

from robot_control_interface.action import Moverobot
from example_action_rclpy.robot import Robot

class ActionRobot(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info('This is a node:{}'.format(name))

        self.robot = Robot()
        self.action_server = ActionServer(self,Moverobot,'move_robot',self.execute_callback)

    def execute_callback(self,goal_handle: ServerGoalHandle):
        self.get_logger().info('execute moving robot')
        self.robot.set_goal(goal_handle.request.distance)
        feedback_msg = Moverobot.Feedback()
        while rclpy.ok() and not self.robot.close_goal():
            # move
            self.robot.move_step()
            feedback_msg.pose = self.robot.get_current_pose()
            feedback_msg.status = self.robot.get_status()
            goal_handle.publish_feedback(feedback_msg)
            if goal_handle.is_cancel_requested:
                result = Moverobot.Result()
                result.pose = self.robot.get_current_pose()
                return result
            time.sleep(0.5)
        goal_handle.succeed()
        result = Moverobot.Result()
        result.pose = self.robot.get_current_pose()
        return result
def main(args=None):
    rclpy.init(args=args)
    node = ActionRobot('action_robot')
    rclpy.spin(node)
    rclpy.shutdown()